/**
 * @file bmi088.c
 * @brief BMI088 IMU驱动实现 - H743-V2飞控板
 * @author Flight Control Team
 * @date 2025-11-09
 *
 * 参考文档:
 *   - reference_firmware/h743_v2_pin_analysis.md
 *   - BMI088数据手册
 *
 * 初始化流程:
 *   1. 加速度计软复位
 *   2. 等待1ms
 *   3. 读取加速度计芯片ID验证
 *   4. 配置加速度计（量程、采样率、电源模式）
 *   5. 陀螺仪软复位
 *   6. 等待30ms
 *   7. 读取陀螺仪芯片ID验证
 *   8. 配置陀螺仪（量程、带宽、电源模式）
 */

#include "bmi088.h"
#include "bsp_spi.h"
#include "stm32h7xx_hal.h"
#include <stddef.h>
#include <math.h>

/* 转换系数 */
#define BMI088_ACCEL_SCALE_24G      (24.0f / 32768.0f * 9.80665f)  /* ±24g → m/s² */
#define BMI088_GYRO_SCALE_2000DPS   (2000.0f / 32768.0f * 0.017453292f)  /* ±2000dps → rad/s */
#define BMI088_TEMP_SCALE           (0.125f)  /* 温度转换系数 */
#define BMI088_TEMP_OFFSET          (23.0f)   /* 温度偏移 */

/* 初始化状态 */
static BMI088_Status_t bmi088_status = {0};

/**
 * @brief 延时函数（毫秒）
 */
static void BMI088_Delay_ms(uint32_t ms)
{
    HAL_Delay(ms);
}

/**
 * @brief 读取加速度计芯片ID
 */
int BMI088_ReadAccelChipID(uint8_t *chip_id)
{
    int ret;

    /* 选中加速度计 */
    BSP_SPI_BMI088_CS(BMI088_ACCEL, 1);

    /* 读取芯片ID */
    ret = BSP_SPI_ReadReg(SPI_PORT_2, BMI088_ACC_CHIP_ID, chip_id);

    /* 释放片选 */
    BSP_SPI_BMI088_CS(BMI088_ACCEL, 0);

    return ret;
}

/**
 * @brief 读取陀螺仪芯片ID
 * 注意：陀螺仪读取不需要dummy字节
 */
int BMI088_ReadGyroChipID(uint8_t *chip_id)
{
    uint8_t tx_buf[2];
    uint8_t rx_buf[2];
    int ret;

    /* 陀螺仪读操作：只需要2字节 */
    tx_buf[0] = BMI088_GYRO_CHIP_ID | 0x80;
    tx_buf[1] = 0x00;

    /* 选中陀螺仪 */
    BSP_SPI_BMI088_CS(BMI088_GYRO, 1);

    /* 读取芯片ID */
    ret = BSP_SPI_Transfer(SPI_PORT_2, tx_buf, rx_buf, 2);

    /* 释放片选 */
    BSP_SPI_BMI088_CS(BMI088_GYRO, 0);

    if (ret == 0) {
        *chip_id = rx_buf[1];
    }

    return ret;
}

/**
 * @brief 写入加速度计寄存器
 */
static int BMI088_WriteAccelReg(uint8_t reg, uint8_t data)
{
    int ret;

    BSP_SPI_BMI088_CS(BMI088_ACCEL, 1);
    ret = BSP_SPI_WriteReg(SPI_PORT_2, reg, data);
    BSP_SPI_BMI088_CS(BMI088_ACCEL, 0);

    return ret;
}

/**
 * @brief 读取加速度计寄存器
 */
static int BMI088_ReadAccelReg(uint8_t reg, uint8_t *data)
{
    int ret;

    BSP_SPI_BMI088_CS(BMI088_ACCEL, 1);
    ret = BSP_SPI_ReadReg(SPI_PORT_2, reg, data);
    BSP_SPI_BMI088_CS(BMI088_ACCEL, 0);

    return ret;
}

/**
 * @brief 写入陀螺仪寄存器
 */
static int BMI088_WriteGyroReg(uint8_t reg, uint8_t data)
{
    int ret;

    BSP_SPI_BMI088_CS(BMI088_GYRO, 1);
    ret = BSP_SPI_WriteReg(SPI_PORT_2, reg, data);
    BSP_SPI_BMI088_CS(BMI088_GYRO, 0);

    return ret;
}

/**
 * @brief 读取陀螺仪寄存器
 * 注意：陀螺仪读取不需要dummy字节，只需要2字节传输（地址 + 数据）
 * 参考：BMI088_Gyroscope.cpp RegisterRead()
 */
static int BMI088_ReadGyroReg(uint8_t reg, uint8_t *data)
{
    uint8_t tx_buf[2];
    uint8_t rx_buf[2];
    int ret;

    /* 陀螺仪读操作：只需要2字节（地址 | 0x80, 数据） */
    tx_buf[0] = reg | 0x80;  /* 读标志 */
    tx_buf[1] = 0x00;        /* 占位字节 */

    BSP_SPI_BMI088_CS(BMI088_GYRO, 1);
    ret = BSP_SPI_Transfer(SPI_PORT_2, tx_buf, rx_buf, 2);
    BSP_SPI_BMI088_CS(BMI088_GYRO, 0);

    if (ret == 0) {
        *data = rx_buf[1];  /* 第二个字节是有效数据 */
    }

    return ret;
}

/**
 * @brief 读取加速度计多个寄存器
 */
static int BMI088_ReadAccelRegs(uint8_t reg, uint8_t *data, uint16_t len)
{
    int ret;

    BSP_SPI_BMI088_CS(BMI088_ACCEL, 1);
    ret = BSP_SPI_ReadRegs(SPI_PORT_2, reg, data, len);
    BSP_SPI_BMI088_CS(BMI088_ACCEL, 0);

    return ret;
}

/**
 * @brief 读取陀螺仪多个寄存器
 * 注意：陀螺仪读取不需要dummy字节
 */
static int BMI088_ReadGyroRegs(uint8_t reg, uint8_t *data, uint16_t len)
{
    uint8_t tx_buf[32];
    uint8_t rx_buf[32];
    int ret;
    uint16_t i;

    /* 检查长度 */
    if (len > 30) {
        return -1;
    }

    /* 准备发送缓冲区：陀螺仪只需要地址 + 数据，不需要dummy字节 */
    tx_buf[0] = reg | 0x80;  /* 读标志 */
    for (i = 1; i <= len; i++) {
        tx_buf[i] = 0x00;  /* 占位字节 */
    }

    BSP_SPI_BMI088_CS(BMI088_GYRO, 1);
    ret = BSP_SPI_Transfer(SPI_PORT_2, tx_buf, rx_buf, len + 1);
    BSP_SPI_BMI088_CS(BMI088_GYRO, 0);

    if (ret == 0) {
        /* 复制数据（跳过第一个字节） */
        for (i = 0; i < len; i++) {
            data[i] = rx_buf[i + 1];
        }
    }

    return ret;
}

/**
 * @brief 初始化加速度计
 */
static int BMI088_InitAccel(void)
{
    uint8_t chip_id = 0;
    uint8_t data;
    int retry = 0;

    /* BMI088加速度计特殊要求：
     * 上电后总是以I2C模式启动，需要通过在CSB1引脚上发送上升沿来切换到SPI模式。
     * 方法：执行一次dummy SPI读取操作（获得的值无效）
     * 参考：BMI088数据手册 6.1 Serial Peripheral Interface (SPI)
     */
    BMI088_ReadAccelChipID(&chip_id);  /* Dummy read to switch to SPI mode */
    BMI088_Delay_ms(1);

    /* 软复位 */
    if (BMI088_WriteAccelReg(BMI088_ACC_SOFTRESET, BMI088_ACC_SOFTRESET_VALUE) != 0) {
        return -1;
    }
    BMI088_Delay_ms(50);  /* 软复位后需要等待足够长的时间 */

    /* 软复位后芯片会回到I2C模式，需要再次执行dummy读取切换到SPI模式 */
    BMI088_ReadAccelChipID(&chip_id);  /* Dummy read after reset */
    BMI088_Delay_ms(1);

    /* 读取芯片ID验证（最多重试10次） */
    for (retry = 0; retry < 10; retry++) {
        if (BMI088_ReadAccelChipID(&chip_id) == 0) {
            if (chip_id == BMI088_ACC_CHIP_ID_VALUE) {
                bmi088_status.accel_chip_id = chip_id;
                break;
            }
        }
        BMI088_Delay_ms(10);
    }

    if (chip_id != BMI088_ACC_CHIP_ID_VALUE) {
        return -1;  /* 芯片ID不匹配 */
    }

    /* 配置电源模式 - 激活模式 */
    if (BMI088_WriteAccelReg(BMI088_ACC_PWR_CONF, BMI088_ACC_PWR_ACTIVE) != 0) {
        return -1;
    }
    BMI088_Delay_ms(10);  /* 参考固件等待10ms */

    /* 使能加速度计 */
    if (BMI088_WriteAccelReg(BMI088_ACC_PWR_CTRL, BMI088_ACC_PWR_ON) != 0) {
        return -1;
    }
    BMI088_Delay_ms(50);  /* 等待加速度计完全启动 */

    /* 配置量程: ±24g */
    if (BMI088_WriteAccelReg(BMI088_ACC_RANGE, BMI088_ACC_RANGE_24G) != 0) {
        return -1;
    }
    BMI088_Delay_ms(50);  /* 等待配置生效 - 需要较长延时 */

    /* 配置采样率和滤波器: 1600Hz, Normal滤波 */
    if (BMI088_WriteAccelReg(BMI088_ACC_CONF, BMI088_ACC_CONF_1600HZ) != 0) {
        return -1;
    }
    BMI088_Delay_ms(50);  /* 等待配置生效 - 需要较长延时 */

    /* 验证配置 */
    if (BMI088_ReadAccelReg(BMI088_ACC_RANGE, &data) != 0) {
        return -1;
    }
    if (data != BMI088_ACC_RANGE_24G) {
        return -1;
    }

    bmi088_status.accel_initialized = 1;
    return 0;
}

/**
 * @brief 初始化陀螺仪
 */
static int BMI088_InitGyro(void)
{
    uint8_t chip_id = 0;
    uint8_t data;
    int retry = 0;

    /* 陀螺仪可能也需要dummy读取来切换到SPI模式（虽然数据手册没有明确说明） */
    BMI088_ReadGyroChipID(&chip_id);  /* Dummy read */
    BMI088_Delay_ms(1);

    /* 软复位 */
    if (BMI088_WriteGyroReg(BMI088_GYRO_SOFTRESET, BMI088_GYRO_SOFTRESET_VALUE) != 0) {
        return -1;
    }
    BMI088_Delay_ms(80);  /* 陀螺仪复位需要至少30ms，增加到80ms确保稳定 */

    /* 软复位后可能需要再次dummy读取 */
    BMI088_ReadGyroChipID(&chip_id);  /* Dummy read after reset */
    BMI088_Delay_ms(1);

    /* 读取芯片ID验证（最多重试10次） */
    for (retry = 0; retry < 10; retry++) {
        if (BMI088_ReadGyroChipID(&chip_id) == 0) {
            if (chip_id == BMI088_GYRO_CHIP_ID_VALUE) {
                bmi088_status.gyro_chip_id = chip_id;
                break;
            }
        }
        BMI088_Delay_ms(10);
    }

    if (chip_id != BMI088_GYRO_CHIP_ID_VALUE) {
        return -1;  /* 芯片ID不匹配 */
    }

    /* 等待10ms再进入配置阶段（参考固件） */
    BMI088_Delay_ms(10);

    /* 配置量程: ±2000 dps */
    if (BMI088_WriteGyroReg(BMI088_GYRO_RANGE, BMI088_GYRO_RANGE_2000DPS) != 0) {
        return -1;
    }
    BMI088_Delay_ms(50);  /* 等待配置生效 - 需要较长延时 */

    /* 配置带宽: 2000Hz, Normal滤波 */
    if (BMI088_WriteGyroReg(BMI088_GYRO_BANDWIDTH, BMI088_GYRO_BW_2000HZ) != 0) {
        return -1;
    }
    BMI088_Delay_ms(50);  /* 等待配置生效 - 需要较长延时 */

    /* 配置电源模式: 正常模式 */
    if (BMI088_WriteGyroReg(BMI088_GYRO_LPM1, BMI088_GYRO_NORMAL_MODE) != 0) {
        return -1;
    }
    BMI088_Delay_ms(50);  /* 等待配置生效 - 需要较长延时 */

    /* 验证配置 */
    if (BMI088_ReadGyroReg(BMI088_GYRO_RANGE, &data) != 0) {
        return -1;
    }
    if (data != BMI088_GYRO_RANGE_2000DPS) {
        return -1;
    }

    bmi088_status.gyro_initialized = 1;
    return 0;
}

/**
 * @brief 初始化BMI088传感器
 */
int BMI088_Init(void)
{
    /* 初始化加速度计 */
    if (BMI088_InitAccel() != 0) {
        return -1;
    }

    /* 初始化陀螺仪 */
    if (BMI088_InitGyro() != 0) {
        return -1;
    }

    return 0;
}

/**
 * @brief 读取BMI088原始数据（带调试信息）
 */
int BMI088_ReadRawData(BMI088_RawData_t *raw_data)
{
    uint8_t buf[6];
    uint8_t status;

    if (raw_data == NULL) {
        return -1;
    }

    /* 检查初始化状态 */
    if (!bmi088_status.accel_initialized || !bmi088_status.gyro_initialized) {
        return -1;
    }

    /* 检查加速度计数据就绪标志 (STATUS寄存器bit 7) */
    /* 暂时注释掉，先确保基本功能正常
    if (BMI088_ReadAccelReg(BMI088_ACC_STATUS, &status) != 0) {
        return -1;
    }

    if (!(status & 0x80)) {
        return -2;
    }
    */

    /* 读取加速度计数据 (6字节: X_LSB, X_MSB, Y_LSB, Y_MSB, Z_LSB, Z_MSB) */
    if (BMI088_ReadAccelRegs(BMI088_ACC_X_LSB, buf, 6) != 0) {
        return -1;
    }

    /* 保存原始字节数据用于调试 */
    raw_data->accel_raw_bytes[0] = buf[0];
    raw_data->accel_raw_bytes[1] = buf[1];
    raw_data->accel_raw_bytes[2] = buf[2];
    raw_data->accel_raw_bytes[3] = buf[3];
    raw_data->accel_raw_bytes[4] = buf[4];
    raw_data->accel_raw_bytes[5] = buf[5];

    /* 组合16位数据（小端格式：LSB在前，MSB在后） */
    raw_data->accel_x_raw = (int16_t)((buf[1] << 8) | buf[0]);
    raw_data->accel_y_raw = (int16_t)((buf[3] << 8) | buf[2]);
    raw_data->accel_z_raw = (int16_t)((buf[5] << 8) | buf[4]);

    /* 读取陀螺仪数据 (6字节: X_LSB, X_MSB, Y_LSB, Y_MSB, Z_LSB, Z_MSB) */
    if (BMI088_ReadGyroRegs(BMI088_GYRO_X_LSB, buf, 6) != 0) {
        return -1;
    }

    /* 组合16位数据（小端格式） */
    raw_data->gyro_x_raw = (int16_t)((buf[1] << 8) | buf[0]);
    raw_data->gyro_y_raw = (int16_t)((buf[3] << 8) | buf[2]);
    raw_data->gyro_z_raw = (int16_t)((buf[5] << 8) | buf[4]);

    /* 读取温度数据 (2字节: TEMP_MSB, TEMP_LSB) */
    /* 注意: BMI088温度数据是11位有符号数
     * 参考：BMI088数据手册 5.3.7 和 PX4固件 BMI088_Accelerometer.cpp:598
     * 公式：Temp_uint11 = (TEMP_MSB * 8) + (TEMP_LSB / 32)
     */
    if (BMI088_ReadAccelRegs(BMI088_ACC_TEMP_MSB, buf, 2) != 0) {
        return -1;
    }

    /* 保存原始字节数据用于调试 */
    raw_data->temp_raw_bytes[0] = buf[0];
    raw_data->temp_raw_bytes[1] = buf[1];

    /* 组合11位温度数据（按照数据手册公式） */
    uint16_t temp_uint11 = (buf[0] * 8) + (buf[1] / 32);

    /* 转换为有符号数（11位，范围0-2047） */
    if (temp_uint11 > 1023) {  /* 大于11位最大正值 */
        raw_data->temp_raw = temp_uint11 - 2048;  /* 转换为负数 */
    } else {
        raw_data->temp_raw = temp_uint11;
    }

    return 0;
}

/**
 * @brief 读取BMI088数据（转换为物理单位）
 */
int BMI088_ReadData(BMI088_Data_t *data)
{
    BMI088_RawData_t raw_data;

    if (data == NULL) {
        return -1;
    }

    /* 读取原始数据 */
    if (BMI088_ReadRawData(&raw_data) != 0) {
        return -1;
    }

    /* 转换加速度数据 (m/s²) */
    data->accel_x = (float)raw_data.accel_x_raw * BMI088_ACCEL_SCALE_24G;
    data->accel_y = (float)raw_data.accel_y_raw * BMI088_ACCEL_SCALE_24G;
    data->accel_z = (float)raw_data.accel_z_raw * BMI088_ACCEL_SCALE_24G;

    /* 转换陀螺仪数据 (rad/s) */
    data->gyro_x = (float)raw_data.gyro_x_raw * BMI088_GYRO_SCALE_2000DPS;
    data->gyro_y = (float)raw_data.gyro_y_raw * BMI088_GYRO_SCALE_2000DPS;
    data->gyro_z = (float)raw_data.gyro_z_raw * BMI088_GYRO_SCALE_2000DPS;

    /* 转换温度数据 (°C) */
    data->temperature = (float)raw_data.temp_raw * BMI088_TEMP_SCALE + BMI088_TEMP_OFFSET;

    /* 获取时间戳 */
    data->timestamp = HAL_GetTick();

    return 0;
}

/**
 * @brief 获取BMI088初始化状态
 */
void BMI088_GetStatus(BMI088_Status_t *status)
{
    if (status != NULL) {
        *status = bmi088_status;
    }
}

/* ========== FIFO模式实现 ========== */

/**
 * @brief 初始化BMI088 FIFO模式
 * @param accel_watermark 加速度计FIFO水印（样本数，0-146）
 * @param gyro_watermark 陀螺仪FIFO水印（样本数，0-100）
 * @return 0: 成功, -1: 失败
 */
int BMI088_FIFO_Init(uint8_t accel_watermark, uint8_t gyro_watermark)
{
    uint8_t reg_val;

    /* ========== 配置加速度计FIFO ========== */

    /* 1. 设置FIFO水印（字节数 = 样本数 × 7） */
    uint16_t accel_wtm_bytes = accel_watermark * BMI088_FIFO_FRAME_SIZE_ACCEL;
    if (accel_wtm_bytes > BMI088_FIFO_SIZE_ACCEL) {
        accel_wtm_bytes = BMI088_FIFO_SIZE_ACCEL;
    }

    /* 写入水印低字节 */
    reg_val = (uint8_t)(accel_wtm_bytes & 0xFF);
    if (BMI088_WriteAccelReg(BMI088_FIFO_WTM_0, reg_val) != 0) {
        return -1;
    }

    /* 写入水印高字节（只有低2位有效） */
    reg_val = (uint8_t)((accel_wtm_bytes >> 8) & 0x03);
    if (BMI088_WriteAccelReg(BMI088_FIFO_WTM_1, reg_val) != 0) {
        return -1;
    }

    /* 2. 配置FIFO_CONFIG_0: Bit1=1(必须), Bit0=1(FIFO模式) */
    reg_val = BMI088_FIFO_CONFIG_0_MODE;
    if (BMI088_WriteAccelReg(BMI088_FIFO_CONFIG_0, reg_val) != 0) {
        return -1;
    }

    /* 3. 配置FIFO_CONFIG_1: Bit6=1(使能加速度计), Bit4=1(必须) */
    reg_val = BMI088_FIFO_CONFIG_1_ACC_EN;
    if (BMI088_WriteAccelReg(BMI088_FIFO_CONFIG_1, reg_val) != 0) {
        return -1;
    }

    /* 4. 配置INT1引脚为输出 */
    reg_val = BMI088_INT1_OUT;
    if (BMI088_WriteAccelReg(BMI088_ACC_INT1_IO_CONF, reg_val) != 0) {
        return -1;
    }

    /* 5. 映射FIFO水印中断到INT1 */
    reg_val = BMI088_INT1_FWM;
    if (BMI088_WriteAccelReg(BMI088_ACC_INT1_INT2_MAP_DATA, reg_val) != 0) {
        return -1;
    }

    /* ========== 配置陀螺仪FIFO ========== */

    /* 1. 使能FIFO中断 */
    reg_val = BMI088_GYRO_INT_CTRL_FIFO_EN;
    if (BMI088_WriteGyroReg(BMI088_GYRO_INT_CTRL, reg_val) != 0) {
        return -1;
    }

    /* 2. 设置FIFO水印（帧数，0-100） */
    if (gyro_watermark > 100) {
        gyro_watermark = 100;
    }
    reg_val = gyro_watermark & 0x7F;  /* 只有低7位有效 */
    if (BMI088_WriteGyroReg(BMI088_GYRO_FIFO_CONFIG_0, reg_val) != 0) {
        return -1;
    }

    /* 3. 使能FIFO水印中断 */
    reg_val = BMI088_GYRO_FIFO_WM_EN;
    if (BMI088_WriteGyroReg(BMI088_GYRO_FIFO_WM_ENABLE, reg_val) != 0) {
        return -1;
    }

    /* 4. 配置FIFO模式 */
    reg_val = BMI088_GYRO_FIFO_MODE;
    if (BMI088_WriteGyroReg(BMI088_GYRO_FIFO_CONFIG_1, reg_val) != 0) {
        return -1;
    }

    /* 5. 配置INT3引脚为推挽输出，高电平有效 */
    /* 读取当前配置 */
    if (BMI088_ReadGyroReg(BMI088_GYRO_INT3_INT4_IO_CONF, &reg_val) != 0) {
        return -1;
    }
    /* 清除Int3_od (Bit1) 和 Int3_lvl (Bit0)，配置为推挽输出，高电平有效 */
    reg_val &= ~0x03;  /* 清除Bit1和Bit0 */
    if (BMI088_WriteGyroReg(BMI088_GYRO_INT3_INT4_IO_CONF, reg_val) != 0) {
        return -1;
    }

    /* 6. 映射FIFO中断到INT3引脚 */
    reg_val = BMI088_GYRO_INT3_FIFO;
    if (BMI088_WriteGyroReg(BMI088_GYRO_INT3_INT4_IO_MAP, reg_val) != 0) {
        return -1;
    }

    return 0;
}

/**
 * @brief 读取陀螺仪FIFO状态
 * @return FIFO帧数，-1表示失败，100表示溢出
 */
int BMI088_FIFO_GetGyroStatus(void)
{
    uint8_t status;

    /* 读取FIFO状态寄存器 */
    if (BMI088_ReadGyroReg(BMI088_GYRO_FIFO_STATUS, &status) != 0) {
        return -1;
    }

    /* 检查溢出标志 */
    if (status & 0x80) {
        /* FIFO溢出 - 不复位，直接读取数据并清除溢出标志 */
        /* 返回100帧（FIFO满） */
        return 100;
    }

    /* 返回帧数（低7位） */
    return (int)(status & 0x7F);
}

/**
 * @brief 读取加速度计FIFO数据（PX4风格：一次性读取长度+数据）
 * @param fifo_data FIFO数据结构指针
 * @return 读取的样本数，-1表示失败，0表示FIFO空
 */
int BMI088_FIFO_ReadAccel(BMI088_FIFO_AccelData_t *fifo_data)
{
    if (fifo_data == NULL) {
        return -1;
    }

    /*
     * PX4方法：一次性读取FIFO_LENGTH和FIFO_DATA
     * 这样可以避免在读取长度和读取数据之间FIFO内容发生变化
     *
     * 传输格式（加速度计需要dummy字节）：
     * TX: [FIFO_LENGTH_0|0x80] [dummy] [0x00] [0x00] [0x00] ...
     * RX: [dummy]              [dummy] [LEN_L] [LEN_H] [DATA0] [DATA1] ...
     */

    /* 计算最大传输大小：4字节头（cmd+dummy+len_l+len_h）+ 最大FIFO数据 */
    const uint16_t max_transfer = 4 + (BMI088_FIFO_MAX_SAMPLES * BMI088_FIFO_FRAME_SIZE_ACCEL);
    uint8_t tx_buf[4 + BMI088_FIFO_MAX_SAMPLES * BMI088_FIFO_FRAME_SIZE_ACCEL];
    uint8_t rx_buf[4 + BMI088_FIFO_MAX_SAMPLES * BMI088_FIFO_FRAME_SIZE_ACCEL];

    /* 准备发送缓冲区 */
    tx_buf[0] = BMI088_FIFO_LENGTH_0 | 0x80;  /* 从FIFO_LENGTH_0开始读取 */
    tx_buf[1] = 0x00;  /* dummy字节 */
    for (uint16_t i = 2; i < max_transfer; i++) {
        tx_buf[i] = 0x00;
    }

    /* 一次性读取FIFO长度和数据 */
    BSP_SPI_BMI088_CS(BMI088_ACCEL, 1);
    int ret = BSP_SPI_Transfer(SPI_PORT_2, tx_buf, rx_buf, max_transfer);
    BSP_SPI_BMI088_CS(BMI088_ACCEL, 0);

    if (ret != 0) {
        return -1;
    }

    /* 提取FIFO长度（rx_buf[2]和rx_buf[3]） */
    uint16_t fifo_length = ((uint16_t)(rx_buf[3] & 0x3F) << 8) | rx_buf[2];

    /* 检查FIFO空标志 */
    if (fifo_length == 0x8000 || fifo_length == 0) {
        return 0;  /* FIFO为空 */
    }

    /* 检查FIFO溢出 */
    if (fifo_length >= BMI088_FIFO_SIZE_ACCEL) {
        return -2;  /* FIFO溢出 */
    }

    /* 解析FIFO数据（从rx_buf[4]开始） */
    fifo_data->accel_count = 0;
    uint16_t index = 4;  /* 跳过cmd+dummy+len_l+len_h */
    uint16_t end_index = 4 + fifo_length;

    if (end_index > max_transfer) {
        end_index = max_transfer;
    }

    while (index < end_index && fifo_data->accel_count < BMI088_FIFO_MAX_SAMPLES) {
        /* 检查帧头 */
        uint8_t header = rx_buf[index];

        if ((header & BMI088_FIFO_HEADER_MASK) == BMI088_FIFO_HEADER_ACC) {
            /* 加速度计数据帧（7字节：1 header + 6 data） */
            if (index + 7 > end_index) {
                break;  /* 数据不完整 */
            }

            /* 提取数据（小端格式） */
            int16_t acc_x = (int16_t)((rx_buf[index + 2] << 8) | rx_buf[index + 1]);
            int16_t acc_y = (int16_t)((rx_buf[index + 4] << 8) | rx_buf[index + 3]);
            int16_t acc_z = (int16_t)((rx_buf[index + 6] << 8) | rx_buf[index + 5]);

            /* 存储数据 */
            fifo_data->accel_x[fifo_data->accel_count] = acc_x;
            fifo_data->accel_y[fifo_data->accel_count] = acc_y;
            fifo_data->accel_z[fifo_data->accel_count] = acc_z;
            fifo_data->accel_count++;

            index += 7;
        }
        else if ((header & BMI088_FIFO_HEADER_MASK) == BMI088_FIFO_HEADER_SKIP) {
            /* 跳帧（2字节） */
            index += 2;
        }
        else if ((header & BMI088_FIFO_HEADER_MASK) == BMI088_FIFO_HEADER_TIME) {
            /* 时间帧（4字节） */
            index += 4;
        }
        else {
            /* 未知帧头，跳过 */
            index++;
        }
    }

    /* 记录时间戳 */
    fifo_data->accel_timestamp = HAL_GetTick();

    return fifo_data->accel_count;
}

/**
 * @brief 读取陀螺仪FIFO数据（PX4风格：先读状态，再读数据）
 * @param fifo_data FIFO数据结构指针
 * @return 读取的样本数，-1表示失败，-2表示溢出，0表示FIFO空
 */
int BMI088_FIFO_ReadGyro(BMI088_FIFO_GyroData_t *fifo_data)
{
    if (fifo_data == NULL) {
        return -1;
    }

    /* 读取FIFO状态 */
    int frame_count = BMI088_FIFO_GetGyroStatus();
    if (frame_count <= 0) {
        return frame_count;  /* 0表示空，-1表示错误，-2表示溢出 */
    }

    /* 限制读取帧数 */
    if (frame_count > BMI088_FIFO_MAX_SAMPLES) {
        frame_count = BMI088_FIFO_MAX_SAMPLES;
    }

    /*
     * 陀螺仪FIFO读取（无dummy字节，2字节协议）
     * TX: [FIFO_DATA|0x80] [0x00] [0x00] ...
     * RX: [dummy]          [DATA0] [DATA1] ...
     */

    /* 计算传输大小：1字节命令 + N帧数据 */
    uint16_t transfer_size = 1 + (frame_count * BMI088_FIFO_FRAME_SIZE_GYRO);
    uint8_t tx_buf[1 + BMI088_FIFO_MAX_SAMPLES * BMI088_FIFO_FRAME_SIZE_GYRO];
    uint8_t rx_buf[1 + BMI088_FIFO_MAX_SAMPLES * BMI088_FIFO_FRAME_SIZE_GYRO];

    /* 准备发送缓冲区 */
    tx_buf[0] = BMI088_GYRO_FIFO_DATA | 0x80;
    for (uint16_t i = 1; i < transfer_size; i++) {
        tx_buf[i] = 0x00;
    }

    /* 读取FIFO数据 */
    BSP_SPI_BMI088_CS(BMI088_GYRO, 1);
    int ret = BSP_SPI_Transfer(SPI_PORT_2, tx_buf, rx_buf, transfer_size);
    BSP_SPI_BMI088_CS(BMI088_GYRO, 0);

    if (ret != 0) {
        return -1;
    }

    /* 解析FIFO数据（从rx_buf[1]开始，陀螺仪FIFO无帧头） */
    fifo_data->gyro_count = 0;

    for (int i = 0; i < frame_count; i++) {
        uint16_t index = 1 + (i * BMI088_FIFO_FRAME_SIZE_GYRO);  /* 跳过命令字节 */

        /* 提取数据（小端格式） */
        int16_t gyro_x = (int16_t)((rx_buf[index + 1] << 8) | rx_buf[index + 0]);
        int16_t gyro_y = (int16_t)((rx_buf[index + 3] << 8) | rx_buf[index + 2]);
        int16_t gyro_z = (int16_t)((rx_buf[index + 5] << 8) | rx_buf[index + 4]);

        /* 过滤无效数据（全0xFFFF或全0x8000） */
        if ((gyro_x == (int16_t)0x8000 && gyro_y == (int16_t)0x8000 && gyro_z == (int16_t)0x8000) ||
            (gyro_x == (int16_t)0xFFFF && gyro_y == (int16_t)0xFFFF && gyro_z == (int16_t)0xFFFF)) {
            continue;  /* 跳过无效帧 */
        }

        /* 存储数据 */
        fifo_data->gyro_x[fifo_data->gyro_count] = gyro_x;
        fifo_data->gyro_y[fifo_data->gyro_count] = gyro_y;
        fifo_data->gyro_z[fifo_data->gyro_count] = gyro_z;
        fifo_data->gyro_count++;
    }

    /* 记录时间戳 */
    fifo_data->gyro_timestamp = HAL_GetTick();

    return fifo_data->gyro_count;
}

/**
 * @brief 复位加速度计FIFO（PX4方法）
 * @return 0: 成功, -1: 失败
 */
int BMI088_FIFO_ResetAccel(void)
{
    /*
     * PX4方法：写0xB0到ACC_SOFTRESET触发FIFO复位
     * 这不会完全复位传感器，只复位FIFO
     */
    if (BMI088_WriteAccelReg(BMI088_ACC_SOFTRESET, 0xB0) != 0) {
        return -1;
    }

    /* 短暂延时等待FIFO复位完成 */
    HAL_Delay(1);

    return 0;
}

/**
 * @brief 复位陀螺仪FIFO（PX4方法）
 * @return 0: 成功, -1: 失败
 */
int BMI088_FIFO_ResetGyro(void)
{
    /*
     * PX4方法：
     * 1. 写0到FIFO_CONFIG_0清空FIFO
     * 2. 写0到FIFO_CONFIG_1禁用FIFO并清除溢出标志
     * 3. 恢复FIFO_CONFIG_0（水印值）
     * 4. 恢复FIFO_CONFIG_1（重新启用FIFO）
     */

    /* 1. 清空FIFO */
    if (BMI088_WriteGyroReg(BMI088_GYRO_FIFO_CONFIG_0, 0x00) != 0) {
        return -1;
    }

    /* 2. 禁用FIFO并清除溢出标志 */
    if (BMI088_WriteGyroReg(BMI088_GYRO_FIFO_CONFIG_1, 0x00) != 0) {
        return -1;
    }

    /* 短暂延时 */
    HAL_Delay(1);

    /* 3. 恢复水印值（假设是20帧，需要从全局变量获取） */
    /* 注意：这里硬编码为20，实际应该保存初始化时的值 */
    if (BMI088_WriteGyroReg(BMI088_GYRO_FIFO_CONFIG_0, 20) != 0) {
        return -1;
    }

    /* 4. 重新启用FIFO模式 */
    if (BMI088_WriteGyroReg(BMI088_GYRO_FIFO_CONFIG_1, BMI088_GYRO_FIFO_MODE) != 0) {
        return -1;
    }

    return 0;
}

/* ========== 优先级2：重要改进功能 ========== */

/* 全局错误统计 */
static BMI088_ErrorStats_t g_bmi088_error_stats = {0};

/* 加速度计寄存器配置表 */
static const BMI088_RegisterConfig_t g_bmi088_accel_register_config[] = {
    /* Register                  | Set bits                         | Clear bits */
    {BMI088_ACC_RANGE,            BMI088_ACC_RANGE_24G,              0x00},  /* ±24g */
    {BMI088_ACC_CONF,             BMI088_ACC_CONF_1600HZ,            0x00},  /* 1600Hz, Normal mode */
    {BMI088_ACC_PWR_CONF,         BMI088_ACC_PWR_ACTIVE,             0x03},  /* Active mode */
    {BMI088_ACC_PWR_CTRL,         BMI088_ACC_PWR_ON,                 0x00},  /* Accel enabled */
    {BMI088_FIFO_CONFIG_1,        BMI088_FIFO_CONFIG_1_ACC_EN,       0x00},  /* FIFO mode enabled */
};

/* 陀螺仪寄存器配置表 */
static const BMI088_RegisterConfig_t g_bmi088_gyro_register_config[] = {
    /* Register                  | Set bits                         | Clear bits */
    {BMI088_GYRO_RANGE,           BMI088_GYRO_RANGE_2000DPS,         0x00},  /* ±2000dps */
    {BMI088_GYRO_BANDWIDTH,       0x01,                              0x00},  /* 2000Hz ODR, 532Hz filter */
    {BMI088_GYRO_LPM1,            0x00,                              0x80},  /* Normal mode */
    {BMI088_GYRO_FIFO_CONFIG_1,   BMI088_GYRO_FIFO_MODE,             0x00},  /* FIFO mode enabled */
};

#define BMI088_ACCEL_REGISTER_CONFIG_COUNT (sizeof(g_bmi088_accel_register_config) / sizeof(g_bmi088_accel_register_config[0]))
#define BMI088_GYRO_REGISTER_CONFIG_COUNT (sizeof(g_bmi088_gyro_register_config) / sizeof(g_bmi088_gyro_register_config[0]))

/**
 * @brief 加速度计寄存器配置验证
 */
int BMI088_Accel_RegisterCheck(const BMI088_RegisterConfig_t *reg_cfg)
{
    if (reg_cfg == NULL) {
        return -1;
    }

    uint8_t reg_value = 0;
    if (BMI088_ReadAccelReg(reg_cfg->reg, &reg_value) != 0) {
        g_bmi088_error_stats.accel_bad_transfer_count++;
        return -1;
    }

    /* 检查应该设置的位 */
    if (reg_cfg->set_bits && ((reg_value & reg_cfg->set_bits) != reg_cfg->set_bits)) {
        g_bmi088_error_stats.accel_bad_register_count++;
        return -1;
    }

    /* 检查应该清除的位 */
    if (reg_cfg->clear_bits && ((reg_value & reg_cfg->clear_bits) != 0)) {
        g_bmi088_error_stats.accel_bad_register_count++;
        return -1;
    }

    return 0;
}

/**
 * @brief 陀螺仪寄存器配置验证
 */
int BMI088_Gyro_RegisterCheck(const BMI088_RegisterConfig_t *reg_cfg)
{
    if (reg_cfg == NULL) {
        return -1;
    }

    uint8_t reg_value = 0;
    if (BMI088_ReadGyroReg(reg_cfg->reg, &reg_value) != 0) {
        g_bmi088_error_stats.gyro_bad_transfer_count++;
        return -1;
    }

    /* 检查应该设置的位 */
    if (reg_cfg->set_bits && ((reg_value & reg_cfg->set_bits) != reg_cfg->set_bits)) {
        g_bmi088_error_stats.gyro_bad_register_count++;
        return -1;
    }

    /* 检查应该清除的位 */
    if (reg_cfg->clear_bits && ((reg_value & reg_cfg->clear_bits) != 0)) {
        g_bmi088_error_stats.gyro_bad_register_count++;
        return -1;
    }

    return 0;
}

/**
 * @brief 读取加速度计温度
 *
 * BMI088加速度计有温度传感器
 * 温度公式：Temperature = temp_raw * 0.125 + 23
 */
int BMI088_Accel_UpdateTemperature(float *temperature)
{
    if (temperature == NULL) {
        return -1;
    }

    /* 读取温度寄存器（2字节，11位有符号数） */
    uint8_t temp_buf[3] = {0};
    if (BMI088_ReadAccelRegs(BMI088_ACC_TEMP_MSB, temp_buf, 2) != 0) {
        g_bmi088_error_stats.accel_bad_transfer_count++;
        return -1;
    }

    /* 组合温度值：11位有符号数，temp_buf[0] = MSB[10:3], temp_buf[1] = LSB[2:0] */
    int16_t temp_raw = ((int16_t)temp_buf[0] << 3) | (temp_buf[1] >> 5);

    /* 扩展符号位（11位 → 16位） */
    if (temp_raw & 0x0400) {  /* 检查第10位（符号位） */
        temp_raw |= 0xF800;   /* 扩展符号位 */
    }

    /* 温度转换公式：Temperature = temp_raw * 0.125 + 23 */
    *temperature = (float)temp_raw * 0.125f + 23.0f;

    return 0;
}

/**
 * @brief 读取陀螺仪温度
 *
 * 注意：BMI088陀螺仪没有温度传感器，返回加速度计温度
 */
int BMI088_Gyro_UpdateTemperature(float *temperature)
{
    /* BMI088陀螺仪没有温度传感器，使用加速度计温度 */
    return BMI088_Accel_UpdateTemperature(temperature);
}

/**
 * @brief 获取错误统计
 */
void BMI088_GetErrorStats(BMI088_ErrorStats_t *stats)
{
    if (stats != NULL) {
        *stats = g_bmi088_error_stats;
    }
}

/**
 * @brief 重置错误统计
 */
void BMI088_ResetErrorStats(void)
{
    g_bmi088_error_stats.accel_bad_register_count = 0;
    g_bmi088_error_stats.accel_bad_transfer_count = 0;
    g_bmi088_error_stats.accel_fifo_empty_count = 0;
    g_bmi088_error_stats.accel_fifo_overflow_count = 0;
    g_bmi088_error_stats.gyro_bad_register_count = 0;
    g_bmi088_error_stats.gyro_bad_transfer_count = 0;
    g_bmi088_error_stats.gyro_fifo_empty_count = 0;
    g_bmi088_error_stats.gyro_fifo_overflow_count = 0;
}

/* ========== 优先级3：高级功能 ========== */

/* 全局状态机变量 */
static BMI088_State_t g_bmi088_state = BMI088_STATE_RESET;
static uint32_t g_bmi088_failure_count = 0;
static uint32_t g_bmi088_reset_timestamp = 0;  /* 复位时间戳（用于超时检测） */

/**
 * @brief 软复位BMI088传感器
 *
 * 模仿PX4设计：分别复位加速度计和陀螺仪
 *
 * @return 0成功，-1失败
 */
int BMI088_Reset(void)
{
    /* 复位加速度计：写入0xB6到ACC_SOFTRESET */
    if (BMI088_WriteAccelReg(BMI088_ACC_SOFTRESET, BMI088_ACC_SOFTRESET_VALUE) != 0) {
        g_bmi088_error_stats.accel_bad_transfer_count++;
        return -1;
    }

    HAL_Delay(1);

    /* 复位陀螺仪：写入0xB6到GYRO_SOFTRESET */
    if (BMI088_WriteGyroReg(BMI088_GYRO_SOFTRESET, BMI088_GYRO_SOFTRESET_VALUE) != 0) {
        g_bmi088_error_stats.gyro_bad_transfer_count++;
        return -1;
    }

    /* 更新状态和时间戳 */
    g_bmi088_state = BMI088_STATE_WAIT_FOR_RESET;
    g_bmi088_reset_timestamp = HAL_GetTick();
    g_bmi088_failure_count = 0;

    return 0;
}

/**
 * @brief 获取当前状态
 */
BMI088_State_t BMI088_GetState(void)
{
    return g_bmi088_state;
}

/**
 * @brief 状态机处理函数
 *
 * 模仿PX4的状态机设计，每次调用推进状态
 *
 * @return 0成功，-1失败，1需要等待
 */
int BMI088_StateMachine(void)
{
    uint32_t now = HAL_GetTick();

    switch (g_bmi088_state) {
    case BMI088_STATE_RESET:
        /* 执行软复位 */
        if (BMI088_Reset() == 0) {
            return 1;  /* 需要等待复位完成 */
        } else {
            return -1;  /* 复位失败 */
        }

    case BMI088_STATE_WAIT_FOR_RESET:
        /* 等待复位完成（至少30ms） */
        if ((now - g_bmi088_reset_timestamp) >= 30) {
            /* 检查CHIP_ID确认复位完成 */
            uint8_t accel_id = 0, gyro_id = 0;
            BMI088_ReadAccelChipID(&accel_id);
            BMI088_ReadGyroChipID(&gyro_id);

            if (accel_id == 0x1E && gyro_id == 0x0F) {
                /* 复位成功，进入配置状态 */
                g_bmi088_state = BMI088_STATE_CONFIGURE;
                return 0;
            } else if ((now - g_bmi088_reset_timestamp) > 1000) {
                /* 超时（1秒），复位失败 */
                g_bmi088_state = BMI088_STATE_RESET;
                g_bmi088_failure_count++;
                return -1;
            } else {
                /* 继续等待 */
                return 1;
            }
        }
        return 1;  /* 继续等待 */

    case BMI088_STATE_CONFIGURE:
        /* 配置传感器（在我们的初始化中已经完成） */
        /* 这里直接进入FIFO复位状态 */
        g_bmi088_state = BMI088_STATE_FIFO_RESET;
        return 0;

    case BMI088_STATE_FIFO_RESET:
        /* 复位FIFO */
        if (BMI088_FIFO_ForceReset() == 0) {
            g_bmi088_state = BMI088_STATE_FIFO_READ;
            g_bmi088_failure_count = 0;  /* 成功，重置失败计数 */
            return 0;
        } else {
            g_bmi088_failure_count++;
            return -1;
        }

    case BMI088_STATE_FIFO_READ:
        /* 正常运行状态，不需要状态转换 */
        return 0;

    default:
        /* 未知状态，复位 */
        g_bmi088_state = BMI088_STATE_RESET;
        return -1;
    }
}

/**
 * @brief 检查FIFO溢出
 *
 * 读取FIFO状态，检查是否溢出
 *
 * @return 0无溢出，1溢出，-1读取失败
 */
int BMI088_FIFO_CheckOverflow(void)
{
    /* 检查陀螺仪FIFO状态 */
    int gyro_status = BMI088_FIFO_GetGyroStatus();
    if (gyro_status < 0) {
        g_bmi088_error_stats.gyro_bad_transfer_count++;
        return -1;
    }

    /* BMI088陀螺仪FIFO大小为100帧 */
    if (gyro_status >= 100) {
        g_bmi088_error_stats.gyro_fifo_overflow_count++;
        return 1;  /* 溢出 */
    }

    /* 注意：BMI088加速度计FIFO没有状态寄存器，无法直接检查溢出 */
    /* 溢出检测依赖于FIFO读取时的错误检测 */

    return 0;  /* 无溢出 */
}

/**
 * @brief 强制复位FIFO
 *
 * 模仿PX4设计：通过禁用和重新启用FIFO来复位
 *
 * @return 0成功，-1失败
 */
int BMI088_FIFO_ForceReset(void)
{
    /* 复位加速度计FIFO */
    if (BMI088_FIFO_ResetAccel() != 0) {
        g_bmi088_error_stats.accel_bad_transfer_count++;
        return -1;
    }

    /* 复位陀螺仪FIFO */
    if (BMI088_FIFO_ResetGyro() != 0) {
        g_bmi088_error_stats.gyro_bad_transfer_count++;
        return -1;
    }

    return 0;
}

/**
 * @brief 增加失败计数
 */
void BMI088_IncrementFailureCount(void)
{
    g_bmi088_failure_count++;
}

/**
 * @brief 重置失败计数
 */
void BMI088_ResetFailureCount(void)
{
    g_bmi088_failure_count = 0;
}

/**
 * @brief 获取失败计数
 */
uint32_t BMI088_GetFailureCount(void)
{
    return g_bmi088_failure_count;
}
